#include "RefinePoints.h"
#include <stack>
#include <QDebug>

RefinePoints::RefinePoints(int pointsCount) : Filter(pointsCount + 1, 1), pointsCount(pointsCount)
{
}


RefinePoints::~RefinePoints(void)
{
}

void RefinePoints::Process()
{
	std::list<cv::Point2i> inputPoints;
	for (int i = 1; i < pointsCount + 1; ++i)
	{
		cv::Point2i inputPt = cv::Point2i(inputs[i]->at<int>(0, 0), inputs[i]->at<int>(0, 1));
		inputPoints.push_back(inputPt);
	}

	outputs[0] = inputs[0]->clone();

	for (auto it = inputPoints.begin(); it != inputPoints.end(); ++it)
	{
		ushort *p = inputs[0]->ptr<ushort>(it->y);

		if (p[it->x])
		{
			continue;
		}

		std::list<cv::Point2i> border;
		std::list<cv::Point2i> points;

		cv::Mat mask = cv::Mat(inputs[0]->rows, inputs[0]->cols, CV_8UC1);
		start(*it, border, points, mask);

		if (border.size() && points.size())
		{
			for (auto pt = points.begin(); pt != points.end(); ++pt)
			{
				double avg = 0;
				double weightSum = 0;
				for (auto it = border.begin(); it != border.end(); ++it)
				{
					double distance = (pt->x - it->x)*(pt->x - it->x) + (pt->y - it->y)*(pt->y - it->y);
					double w = distance;
					avg += inputs[0]->at<ushort>(*it) * w;
					weightSum += w;
				}

				avg /= weightSum;
				ushort val = cv::saturate_cast<ushort>(avg);
				outputs[0].at<ushort>(*pt) = val;
			}
		}
	}	
}

void RefinePoints::start(const cv::Point2i &startPt, std::list<cv::Point2i> &result, std::list<cv::Point2i> &points, cv::Mat &mask)
{
	int minx = INT_MAX;
	int maxx = INT_MIN;
	int miny = INT_MAX;
	int maxy = INT_MIN;

	std::stack<cv::Point2i> toProcess;
	toProcess.push(startPt);

	while (toProcess.size())
	{
		cv::Point2i p = toProcess.top();
		toProcess.pop();

		mask.at<uchar>(p) = 1;
		if (inputs[0]->at<ushort>(p) != 0)
		{
			result.push_back(p);
		}
		else
		{
			points.push_back(p);

			minx = MIN(minx, p.x);
			maxx = MAX(maxx, p.x);
			miny = MIN(miny, p.y);
			maxy = MAX(maxy, p.y);

			cv::Point2i nextPt = cv::Point2i(p.x - 1, p.y);
			if (nextPt.x >= 0 && mask.at<uchar>(nextPt) == 0)
				toProcess.push(nextPt);

			nextPt = cv::Point2i(p.x + 1, p.y);
			if (nextPt.x < inputs[0]->cols && mask.at<uchar>(nextPt) == 0)
				toProcess.push(nextPt);

			nextPt = cv::Point2i(p.x, p.y - 1);
			if (nextPt.y >= 0 && mask.at<uchar>(nextPt) == 0)
				toProcess.push(nextPt);

			nextPt = cv::Point2i(p.x, p.y + 1);
			if (nextPt.y < inputs[0]->rows && mask.at<uchar>(nextPt) == 0)
				toProcess.push(nextPt);
		}
	}
}
